"""遥控小车的遥控器
荔枝派读取GY25的数据，通过蓝牙发送给小车

GY25与荔枝派的连接：
GY25的VCC接3.3V
GY25的GND接GND
GY25的RXD接TXD
GY25的TXD接RXD
"""

from bt import Bluetooth
import GY25
import threading
import time
import os
import serial
import queue

if __name__ == '__main__':
    remote_bluetooth = Bluetooth('98:DA:50:02:C6:26')
    time.sleep(1)
    remote_bluetooth.oled.clear()

    remote_bluetooth.oled.write_text('initializing...\n please wait...\n Do NOT touch the remoter!!!')
    gy = GY25.GY25()
    remote_bluetooth.oled.write_text('initialization success!!!\n You can touch the remoter now!!!')
    time.sleep(1)
    remote_bluetooth.oled.clear()
    
    def main():
        global yaw, pitch, roll, angle, a, sign
        while True:
            a = gy.read_angle()
            # 读取数据,yaw:航向角，pitch:俯仰角，roll:滚动角
            if a is not None:
                yaw, pitch, roll = a

                # 姿态解算
                angle = GY25.calculate_angle(pitch, roll)

                # region处理数据
                if angle < 10:
                    # 静止不动
                    sign = 0

                elif angle < 40:
                    # 慢速移动
                    if pitch < -10:
                        if roll < -10:
                            # 左前
                            sign = 5
                        elif roll > 10:
                            # 右前
                            sign = 7
                        else:
                            # 向前
                            sign = 1
                    elif pitch > 10:
                        if roll < -10:
                            # 左后
                            sign = 11
                        elif roll > 10:
                            # 右后
                            sign = 12
                        else:
                            # 向后
                            sign = 3
                    else:
                        if roll <= -30:
                            # 大幅度向左
                            sign = 9
                        elif -30 < roll < -10:
                            # 左
                            sign = 6
                        elif 10 < roll < 30:
                            # 右
                            sign = 8
                        
                        elif roll >= 30:
                            # 大幅度向右
                            sign = 10
                        else:
                            # 停止
                            sign = 0


                else:
                    # 快速移动
                    if pitch < -10 and -10 < roll < 10:
                        # 向前
                        sign = 2
                    elif pitch > 10 and -10 < roll < 10:
                        # 向后
                        sign = 4

                # endregion
                
                os.system('clear')
                print(f'偏航角:{yaw}\n俯仰角:{pitch}\n滚动角:{roll}')
                print(f'角度:{angle}')

            else:
                continue
    def write():
    # 在画布上绘制文本
        while True:
            try:
                exception = gy.exception_queue.get_nowait()
                raise serial.SerialTimeoutException
            except queue.Empty:
                pass
            except serial.SerialTimeoutException:
                gy.oled.clear()
                gy.oled.write_text('GY25 timeoutERROR!!!')
                threading.Thread(target=write,daemon=True)
                time.sleep(0.01)
                continue

            try:
                time.sleep(0.01)
                gy.oled.write_text(f' yaw: {yaw}\n pitch:{pitch}\n roll:{roll}\n angle:{angle}')
            except NameError:
                continue

    def send():
        while True:
            try:
                # 发送信号
                time.sleep(0.05)
                remote_bluetooth.send(sign)
            except:
                continue
        

    threading.Thread(target=main).start()
    threading.Thread(target=write,daemon=True).start()
    threading.Thread(target=send,daemon=True).start()